//M*//////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

/************************************************************************************\
    This is improved variant of chessboard corner detection algorithm that
    uses a graph of connected quads. It is based on the code contributed
    by Vladimir Vezhnevets and Philip Gruebele.
    Here is the copyright notice from the original Vladimir's code:
    ===============================================================

    The algorithms developed and implemented by Vezhnevets Vldimir
    aka Dead Moroz (vvp@graphics.cs.msu.ru)
    See http://graphics.cs.msu.su/en/research/calibration/opencv.html
    for detailed information.

    Reliability additions and modifications made by Philip Gruebele.
    <a href="mailto:pgruebele@cox.net">pgruebele@cox.net</a>

    Some further improvements for detection of partially ocluded boards at non-ideal
    lighting conditions have been made by Alex Bovyrin and Kurt Kolonige

\************************************************************************************/

#include "precomp.hpp"
#include <stdarg.h>

//#define ENABLE_TRIM_COL_ROW

//#pragma comment(lib, "highgui200d.lib")
//#define DEBUG_CHESSBOARD
#ifdef DEBUG_CHESSBOARD
static int PRINTF(const char* fmt, ...) {
    va_list args;
    va_start(args, fmt);
    return vprintf(fmt, args);
}
#include "..//..//include/opencv/highgui.h"
#else
static int PRINTF(const char*, ...) {
    return 0;
}
#endif


//=====================================================================================
// Implementation for the enhanced calibration object detection
//=====================================================================================

#define MAX_CONTOUR_APPROX  7

struct CvContourEx {
    CV_CONTOUR_FIELDS()
    int counter;
};

//=====================================================================================

/// Corner info structure
/** This structure stores information about the chessboard corner.*/
struct CvCBCorner {
    CvPoint2D32f pt; // Coordinates of the corner
    int row;         // Board row index
    int count;       // Number of neighbor corners
    struct CvCBCorner* neighbors[4]; // Neighbor corners

    float meanDist(int* _n) const {
        float sum = 0;
        int n = 0;
        for (int i = 0; i < 4; i++) {
            if (neighbors[i]) {
                float dx = neighbors[i]->pt.x - pt.x;
                float dy = neighbors[i]->pt.y - pt.y;
                sum += sqrt(dx * dx + dy * dy);
                n++;
            }
        }
        if (_n) {
            *_n = n;
        }
        return sum / MAX(n, 1);
    }
};

//=====================================================================================
/// Quadrangle contour info structure
/** This structure stores information about the chessboard quadrange.*/
struct CvCBQuad {
    int count;      // Number of quad neighbors
    int group_idx;  // quad group ID
    int row, col;   // row and column of this quad
    bool ordered;   // true if corners/neighbors are ordered counter-clockwise
    float edge_len; // quad edge len, in pix^2
    // neighbors and corners are synced, i.e., neighbor 0 shares corner 0
    CvCBCorner* corners[4]; // Coordinates of quad corners
    struct CvCBQuad* neighbors[4]; // Pointers of quad neighbors
};

//=====================================================================================

//static CvMat* debug_img = 0;

static int icvGenerateQuads(CvCBQuad** quads, CvCBCorner** corners,
                            CvMemStorage* storage, CvMat* image, int flags);

/*static int
icvGenerateQuadsEx( CvCBQuad **out_quads, CvCBCorner **out_corners,
    CvMemStorage *storage, CvMat *image, CvMat *thresh_img, int dilation, int flags );*/

static void icvFindQuadNeighbors(CvCBQuad* quads, int quad_count);

static int icvFindConnectedQuads(CvCBQuad* quads, int quad_count,
                                 CvCBQuad** quad_group, int group_idx,
                                 CvMemStorage* storage);

static int icvCheckQuadGroup(CvCBQuad** quad_group, int count,
                             CvCBCorner** out_corners, CvSize pattern_size);

static int icvCleanFoundConnectedQuads(int quad_count,
                                       CvCBQuad** quads, CvSize pattern_size);

static int icvOrderFoundConnectedQuads(int quad_count, CvCBQuad** quads,
                                       int* all_count, CvCBQuad** all_quads, CvCBCorner** corners,
                                       CvSize pattern_size, CvMemStorage* storage);

static void icvOrderQuad(CvCBQuad* quad, CvCBCorner* corner, int common);

#ifdef ENABLE_TRIM_COL_ROW
static int icvTrimCol(CvCBQuad** quads, int count, int col, int dir);

static int icvTrimRow(CvCBQuad** quads, int count, int row, int dir);
#endif

static int icvAddOuterQuad(CvCBQuad* quad, CvCBQuad** quads, int quad_count,
                           CvCBQuad** all_quads, int all_count, CvCBCorner** corners);

static void icvRemoveQuadFromGroup(CvCBQuad** quads, int count, CvCBQuad* q0);

static int icvCheckBoardMonotony(CvPoint2D32f* corners, CvSize pattern_size);

#if 0
static void
icvCalcAffineTranf2D32f(CvPoint2D32f* pts1, CvPoint2D32f* pts2, int count, CvMat* affine_trans) {
    int i, j;
    int real_count = 0;
    for (j = 0; j < count; j++) {
        if (pts1[j].x >= 0) { real_count++; }
    }
    if (real_count < 3) { return; }
    cv::Ptr<CvMat> xy = cvCreateMat(2 * real_count, 6, CV_32FC1);
    cv::Ptr<CvMat> uv = cvCreateMat(2 * real_count, 1, CV_32FC1);
    //estimate affine transfromation
    for (i = 0, j = 0; j < count; j++) {
        if (pts1[j].x >= 0) {
            CV_MAT_ELEM(*xy, float, i * 2 + 1, 2) = CV_MAT_ELEM(*xy, float, i * 2, 0) = pts2[j].x;
            CV_MAT_ELEM(*xy, float, i * 2 + 1, 3) = CV_MAT_ELEM(*xy, float, i * 2, 1) = pts2[j].y;
            CV_MAT_ELEM(*xy, float, i * 2, 2) = CV_MAT_ELEM(*xy, float, i * 2, 3) = CV_MAT_ELEM(*xy, float, i * 2, 5) = \
                                                CV_MAT_ELEM(*xy, float, i * 2 + 1, 0) = CV_MAT_ELEM(*xy, float, i * 2 + 1, 1) = CV_MAT_ELEM(*xy, float, i * 2 + 1, 4) = 0;
            CV_MAT_ELEM(*xy, float, i * 2, 4) = CV_MAT_ELEM(*xy, float, i * 2 + 1, 5) = 1;
            CV_MAT_ELEM(*uv, float, i * 2, 0) = pts1[j].x;
            CV_MAT_ELEM(*uv, float, i * 2 + 1, 0) = pts1[j].y;
            i++;
        }
    }

    cvSolve(xy, uv, affine_trans, CV_SVD);
}
#endif

CV_IMPL
int cvFindChessboardCorners(const void* arr, CvSize pattern_size,
                            CvPoint2D32f* out_corners, int* out_corner_count,
                            int flags) {
    int found = 0;
    CvCBQuad* quads = 0, **quad_group = 0;
    CvCBCorner* corners = 0, **corner_group = 0;

    try {
        int k = 0;
        const int min_dilations = 0;
        const int max_dilations = 7;
        cv::Ptr<CvMat> norm_img, thresh_img;
#ifdef DEBUG_CHESSBOARD
        cv::Ptr<IplImage> dbg_img;
        cv::Ptr<IplImage> dbg1_img;
        cv::Ptr<IplImage> dbg2_img;
#endif
        cv::Ptr<CvMemStorage> storage;

        CvMat stub, *img = (CvMat*)arr;

        int expected_corners_num = (pattern_size.width / 2 + 1) * (pattern_size.height / 2 + 1);

        int prev_sqr_size = 0;

        if (out_corner_count) {
            *out_corner_count = 0;
        }

        IplImage _img;
        int check_chessboard_result;
        int quad_count = 0, group_idx = 0, i = 0, dilations = 0;

        img = cvGetMat(img, &stub);
        //debug_img = img;

        if (CV_MAT_DEPTH(img->type) != CV_8U || CV_MAT_CN(img->type) == 2) {
            CV_Error(CV_StsUnsupportedFormat, "Only 8-bit grayscale or color images are supported");
        }

        if (pattern_size.width <= 2 || pattern_size.height <= 2) {
            CV_Error(CV_StsOutOfRange, "Both width and height of the pattern should have bigger than 2");
        }

        if (!out_corners) {
            CV_Error(CV_StsNullPtr, "Null pointer to corners");
        }

        storage = cvCreateMemStorage(0);
        thresh_img = cvCreateMat(img->rows, img->cols, CV_8UC1);

#ifdef DEBUG_CHESSBOARD
        dbg_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
        dbg1_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
        dbg2_img = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
#endif

        if (CV_MAT_CN(img->type) != 1 || (flags & CV_CALIB_CB_NORMALIZE_IMAGE)) {
            // equalize the input image histogram -
            // that should make the contrast between "black" and "white" areas big enough
            norm_img = cvCreateMat(img->rows, img->cols, CV_8UC1);

            if (CV_MAT_CN(img->type) != 1) {
                cvCvtColor(img, norm_img, CV_BGR2GRAY);
                img = norm_img;
            }

            if (flags & CV_CALIB_CB_NORMALIZE_IMAGE) {
                cvEqualizeHist(img, norm_img);
                img = norm_img;
            }
        }

        if (flags & CV_CALIB_CB_FAST_CHECK) {
            cvGetImage(img, &_img);
            check_chessboard_result = cvCheckChessboard(&_img, pattern_size);
            if (check_chessboard_result <= 0) {
                return 0;
            }
        }

        // Try our standard "1" dilation, but if the pattern is not found, iterate the whole procedure with higher dilations.
        // This is necessary because some squares simply do not separate properly with a single dilation.  However,
        // we want to use the minimum number of dilations possible since dilations cause the squares to become smaller,
        // making it difficult to detect smaller squares.
        for (k = 0; k < 3; k++) {
            for (dilations = min_dilations; dilations <= max_dilations; dilations++) {
                if (found) {
                    break;    // already found it
                }

                cvFree(&quads);
                cvFree(&corners);

                /*if( k == 1 )
                {
                    //Pattern was not found using binarization
                    // Run multi-level quads extraction
                    // In case one-level binarization did not give enough number of quads
                    CV_CALL( quad_count = icvGenerateQuadsEx( &quads, &corners, storage, img, thresh_img, dilations, flags ));
                    PRINTF("EX quad count: %d/%d\n", quad_count, expected_corners_num);
                }
                else*/
                {
                    // convert the input grayscale image to binary (black-n-white)
                    if (flags & CV_CALIB_CB_ADAPTIVE_THRESH) {
                        int block_size = cvRound(prev_sqr_size == 0 ?
                                                 MIN(img->cols, img->rows) * 0.2 : prev_sqr_size * 2.) | 1;

                        // convert to binary
                        cvAdaptiveThreshold(img, thresh_img, 255,
                                            CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, block_size, k * 5);
                        if (dilations > 0) {
                            cvDilate(thresh_img, thresh_img, 0, dilations - 1);
                        }
                    } else {
                        // Make dilation before the thresholding.
                        // It splits chessboard corners
                        //cvDilate( img, thresh_img, 0, 1 );

                        // empiric threshold level
                        double mean = cvAvg(img).val[0];
                        int thresh_level = cvRound(mean - 10);
                        thresh_level = MAX(thresh_level, 10);

                        cvThreshold(img, thresh_img, thresh_level, 255, CV_THRESH_BINARY);
                        cvDilate(thresh_img, thresh_img, 0, dilations);
                    }

#ifdef DEBUG_CHESSBOARD
                    cvCvtColor(thresh_img, dbg_img, CV_GRAY2BGR);
#endif

                    // So we can find rectangles that go to the edge, we draw a white line around the image edge.
                    // Otherwise FindContours will miss those clipped rectangle contours.
                    // The border color will be the image mean, because otherwise we risk screwing up filters like cvSmooth()...
                    cvRectangle(thresh_img, cvPoint(0, 0), cvPoint(thresh_img->cols - 1,
                                thresh_img->rows - 1), CV_RGB(255, 255, 255), 3, 8);

                    quad_count = icvGenerateQuads(&quads, &corners, storage, thresh_img, flags);

                    PRINTF("Quad count: %d/%d\n", quad_count, expected_corners_num);
                }


#ifdef DEBUG_CHESSBOARD
                cvCopy(dbg_img, dbg1_img);
                cvNamedWindow("all_quads", 1);
                // copy corners to temp array
                for (i = 0; i < quad_count; i++) {
                    for (int k = 0; k < 4; k++) {
                        CvPoint2D32f pt1, pt2;
                        CvScalar color = CV_RGB(30, 255, 30);
                        pt1 = quads[i].corners[k]->pt;
                        pt2 = quads[i].corners[(k + 1) % 4]->pt;
                        pt2.x = (pt1.x + pt2.x) / 2;
                        pt2.y = (pt1.y + pt2.y) / 2;
                        if (k > 0) {
                            color = CV_RGB(200, 200, 0);
                        }
                        cvLine(dbg1_img, cvPointFrom32f(pt1), cvPointFrom32f(pt2), color, 3, 8);
                    }
                }


                cvShowImage("all_quads", (IplImage*)dbg1_img);
                cvWaitKey();
#endif

                if (quad_count <= 0) {
                    continue;
                }

                // Find quad's neighbors
                icvFindQuadNeighbors(quads, quad_count);

                // allocate extra for adding in icvOrderFoundQuads
                cvFree(&quad_group);
                cvFree(&corner_group);
                quad_group = (CvCBQuad**)cvAlloc(sizeof(quad_group[0]) * (quad_count + quad_count / 2));
                corner_group = (CvCBCorner**)cvAlloc(sizeof(corner_group[0]) * (quad_count + quad_count / 2) * 4);

                for (group_idx = 0; ; group_idx++) {
                    int count = 0;
                    count = icvFindConnectedQuads(quads, quad_count, quad_group, group_idx, storage);

                    int icount = count;
                    if (count == 0) {
                        break;
                    }

                    // order the quad corners globally
                    // maybe delete or add some
                    PRINTF("Starting ordering of inner quads\n");
                    count = icvOrderFoundConnectedQuads(count, quad_group, &quad_count, &quads, &corners,
                                                        pattern_size, storage);
                    PRINTF("Orig count: %d  After ordering: %d\n", icount, count);


#ifdef DEBUG_CHESSBOARD
                    cvCopy(dbg_img, dbg2_img);
                    cvNamedWindow("connected_group", 1);
                    // copy corners to temp array
                    for (i = 0; i < quad_count; i++) {
                        if (quads[i].group_idx == group_idx)
                            for (int k = 0; k < 4; k++) {
                                CvPoint2D32f pt1, pt2;
                                CvScalar color = CV_RGB(30, 255, 30);
                                if (quads[i].ordered) {
                                    color = CV_RGB(255, 30, 30);
                                }
                                pt1 = quads[i].corners[k]->pt;
                                pt2 = quads[i].corners[(k + 1) % 4]->pt;
                                pt2.x = (pt1.x + pt2.x) / 2;
                                pt2.y = (pt1.y + pt2.y) / 2;
                                if (k > 0) {
                                    color = CV_RGB(200, 200, 0);
                                }
                                cvLine(dbg2_img, cvPointFrom32f(pt1), cvPointFrom32f(pt2), color, 3, 8);
                            }
                    }
                    cvShowImage("connected_group", (IplImage*)dbg2_img);
                    cvWaitKey();
#endif

                    if (count == 0) {
                        continue;    // haven't found inner quads
                    }


                    // If count is more than it should be, this will remove those quads
                    // which cause maximum deviation from a nice square pattern.
                    count = icvCleanFoundConnectedQuads(count, quad_group, pattern_size);
                    PRINTF("Connected group: %d  orig count: %d cleaned: %d\n", group_idx, icount, count);

                    count = icvCheckQuadGroup(quad_group, count, corner_group, pattern_size);
                    PRINTF("Connected group: %d  count: %d  cleaned: %d\n", group_idx, icount, count);

                    {
                        int n = count > 0 ? pattern_size.width * pattern_size.height : -count;
                        n = MIN(n, pattern_size.width * pattern_size.height);
                        float sum_dist = 0;
                        int total = 0;

                        for (i = 0; i < n; i++) {
                            int ni = 0;
                            float avgi = corner_group[i]->meanDist(&ni);
                            sum_dist += avgi * ni;
                            total += ni;
                        }
                        prev_sqr_size = cvRound(sum_dist / MAX(total, 1));

                        if (count > 0 || (out_corner_count && -count > *out_corner_count)) {
                            // copy corners to output array
                            for (i = 0; i < n; i++) {
                                out_corners[i] = corner_group[i]->pt;
                            }

                            if (out_corner_count) {
                                *out_corner_count = n;
                            }

                            if (count == pattern_size.width * pattern_size.height &&
                                    icvCheckBoardMonotony(out_corners, pattern_size)) {
                                found = 1;
                                break;
                            }
                        }
                    }
                }
            }//dilations
        }//

        if (found) {
            found = icvCheckBoardMonotony(out_corners, pattern_size);
        }

        // check that none of the found corners is too close to the image boundary
        if (found) {
            const int BORDER = 8;
            for (k = 0; k < pattern_size.width * pattern_size.height; k++) {
                if (out_corners[k].x <= BORDER || out_corners[k].x > img->cols - BORDER ||
                        out_corners[k].y <= BORDER || out_corners[k].y > img->rows - BORDER) {
                    break;
                }
            }

            found = k == pattern_size.width * pattern_size.height;
        }

        if (found && pattern_size.height % 2 == 0 && pattern_size.width % 2 == 0) {
            int last_row = (pattern_size.height - 1) * pattern_size.width;
            double dy0 = out_corners[last_row].y - out_corners[0].y;
            if (dy0 < 0) {
                int i, n = pattern_size.width * pattern_size.height;
                for (i = 0; i < n / 2; i++) {
                    CvPoint2D32f temp;
                    CV_SWAP(out_corners[i], out_corners[n - i - 1], temp);
                }
            }
        }

        if (found) {
            cv::Ptr<CvMat> gray;
            if (CV_MAT_CN(img->type) != 1) {
                gray = cvCreateMat(img->rows, img->cols, CV_8UC1);
                cvCvtColor(img, gray, CV_BGR2GRAY);
            } else {
                gray = cvCloneMat(img);
            }
            int wsize = 2;
            cvFindCornerSubPix(gray, out_corners, pattern_size.width * pattern_size.height,
                               cvSize(wsize, wsize), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 15, 0.1));
        }
    } catch (...) {
        cvFree(&quads);
        cvFree(&corners);
        cvFree(&quad_group);
        cvFree(&corner_group);
        throw;
    }

    cvFree(&quads);
    cvFree(&corners);
    cvFree(&quad_group);
    cvFree(&corner_group);
    return found;
}

//
// Checks that each board row and column is pretty much monotonous curve:
// It analyzes each row and each column of the chessboard as following:
//    for each corner c lying between end points in the same row/column it checks that
//    the point projection to the line segment (a,b) is lying between projections
//    of the neighbor corners in the same row/column.
//
// This function has been created as temporary workaround for the bug in current implementation
// of cvFindChessboardCornes that produces absolutely unordered sets of corners.
//

static int
icvCheckBoardMonotony(CvPoint2D32f* corners, CvSize pattern_size) {
    int i, j, k;

    for (k = 0; k < 2; k++) {
        for (i = 0; i < (k == 0 ? pattern_size.height : pattern_size.width); i++) {
            CvPoint2D32f a = k == 0 ? corners[i * pattern_size.width] : corners[i];
            CvPoint2D32f b = k == 0 ? corners[(i + 1) * pattern_size.width - 1] :
                             corners[(pattern_size.height - 1) * pattern_size.width + i];
            float prevt = 0, dx0 = b.x - a.x, dy0 = b.y - a.y;
            if (fabs(dx0) + fabs(dy0) < FLT_EPSILON) {
                return 0;
            }
            for (j = 1; j < (k == 0 ? pattern_size.width : pattern_size.height) - 1; j++) {
                CvPoint2D32f c = k == 0 ? corners[i * pattern_size.width + j] :
                                 corners[j * pattern_size.width + i];
                float t = ((c.x - a.x) * dx0 + (c.y - a.y) * dy0) / (dx0 * dx0 + dy0 * dy0);
                if (t < prevt || t > 1) {
                    return 0;
                }
                prevt = t;
            }
        }
    }

    return 1;
}

//
// order a group of connected quads
// order of corners:
//   0 is top left
//   clockwise from there
// note: "top left" is nominal, depends on initial ordering of starting quad
//   but all other quads are ordered consistently
//
// can change the number of quads in the group
// can add quads, so we need to have quad/corner arrays passed in
//

static int
icvOrderFoundConnectedQuads(int quad_count, CvCBQuad** quads,
                            int* all_count, CvCBQuad** all_quads, CvCBCorner** corners,
                            CvSize pattern_size, CvMemStorage* storage) {
    cv::Ptr<CvMemStorage> temp_storage = cvCreateChildMemStorage(storage);
    CvSeq* stack = cvCreateSeq(0, sizeof(*stack), sizeof(void*), temp_storage);
    int i;

    // first find an interior quad
    CvCBQuad* start = NULL;
    for (i = 0; i < quad_count; i++) {
        if (quads[i]->count == 4) {
            start = quads[i];
            break;
        }
    }

    if (start == NULL) {
        return 0;    // no 4-connected quad
    }

    // start with first one, assign rows/cols
    int row_min = 0, col_min = 0, row_max = 0, col_max = 0;

    std::map<int, int> col_hist;
    std::map<int, int> row_hist;

    cvSeqPush(stack, &start);
    start->row = 0;
    start->col = 0;
    start->ordered = true;

    // Recursively order the quads so that all position numbers (e.g.,
    // 0,1,2,3) are in the at the same relative corner (e.g., lower right).

    while (stack->total) {
        CvCBQuad* q;
        cvSeqPop(stack, &q);
        int col = q->col;
        int row = q->row;
        col_hist[col]++;
        row_hist[row]++;

        // check min/max
        if (row > row_max) { row_max = row; }
        if (row < row_min) { row_min = row; }
        if (col > col_max) { col_max = col; }
        if (col < col_min) { col_min = col; }

        for (int i = 0; i < 4; i++) {
            CvCBQuad* neighbor = q->neighbors[i];
            switch (i) { // adjust col, row for this quad
                // start at top left, go clockwise
            case 0:
                row--; col--; break;
            case 1:
                col += 2; break;
            case 2:
                row += 2;	break;
            case 3:
                col -= 2; break;
            }

            // just do inside quads
            if (neighbor && neighbor->ordered == false && neighbor->count == 4) {
                PRINTF("col: %d  row: %d\n", col, row);
                icvOrderQuad(neighbor, q->corners[i], (i + 2) % 4); // set in order
                neighbor->ordered = true;
                neighbor->row = row;
                neighbor->col = col;
                cvSeqPush(stack, &neighbor);
            }
        }
    }

    for (i = col_min; i <= col_max; i++) {
        PRINTF("HIST[%d] = %d\n", i, col_hist[i]);
    }

    // analyze inner quad structure
    int w = pattern_size.width - 1;
    int h = pattern_size.height - 1;
    int drow = row_max - row_min + 1;
    int dcol = col_max - col_min + 1;

    // normalize pattern and found quad indices
    if ((w > h && dcol < drow) ||
            (w < h && drow < dcol)) {
        h = pattern_size.width - 1;
        w = pattern_size.height - 1;
    }

    PRINTF("Size: %dx%d  Pattern: %dx%d\n", dcol, drow, w, h);

    // check if there are enough inner quads
    if (dcol < w || drow < h) { // found enough inner quads?
        PRINTF("Too few inner quad rows/cols\n");
        return 0;   // no, return
    }
#ifdef ENABLE_TRIM_COL_ROW
    // too many columns, not very common
    if (dcol == w + 1) { // too many, trim
        PRINTF("Trimming cols\n");
        if (col_hist[col_max] > col_hist[col_min]) {
            PRINTF("Trimming left col\n");
            quad_count = icvTrimCol(quads, quad_count, col_min, -1);
        } else {
            PRINTF("Trimming right col\n");
            quad_count = icvTrimCol(quads, quad_count, col_max, +1);
        }
    }

    // too many rows, not very common
    if (drow == h + 1) { // too many, trim
        PRINTF("Trimming rows\n");
        if (row_hist[row_max] > row_hist[row_min]) {
            PRINTF("Trimming top row\n");
            quad_count = icvTrimRow(quads, quad_count, row_min, -1);
        } else {
            PRINTF("Trimming bottom row\n");
            quad_count = icvTrimRow(quads, quad_count, row_max, +1);
        }
    }
#endif

    // check edges of inner quads
    // if there is an outer quad missing, fill it in
    // first order all inner quads
    int found = 0;
    for (i = 0; i < quad_count; i++) {
        if (quads[i]->count == 4) {
            // ok, look at neighbors
            int col = quads[i]->col;
            int row = quads[i]->row;
            for (int j = 0; j < 4; j++) {
                switch (j) { // adjust col, row for this quad
                    // start at top left, go clockwise
                case 0:
                    row--; col--; break;
                case 1:
                    col += 2; break;
                case 2:
                    row += 2;	break;
                case 3:
                    col -= 2; break;
                }
                CvCBQuad* neighbor = quads[i]->neighbors[j];
                if (neighbor && !neighbor->ordered && // is it an inner quad?
                        col <= col_max && col >= col_min &&
                        row <= row_max && row >= row_min) {
                    // if so, set in order
                    PRINTF("Adding inner: col: %d  row: %d\n", col, row);
                    found++;
                    icvOrderQuad(neighbor, quads[i]->corners[j], (j + 2) % 4);
                    neighbor->ordered = true;
                    neighbor->row = row;
                    neighbor->col = col;
                }
            }
        }
    }

    // if we have found inner quads, add corresponding outer quads,
    //   which are missing
    if (found > 0) {
        PRINTF("Found %d inner quads not connected to outer quads, repairing\n", found);
        for (int i = 0; i < quad_count; i++) {
            if (quads[i]->count < 4 && quads[i]->ordered) {
                int added = icvAddOuterQuad(quads[i], quads, quad_count, all_quads, *all_count, corners);
                *all_count += added;
                quad_count += added;
            }
        }
    }


    // final trimming of outer quads
    if (dcol == w && drow == h) {	// found correct inner quads
        PRINTF("Inner bounds ok, check outer quads\n");
        int rcount = quad_count;
        for (int i = quad_count - 1; i >= 0; i--) // eliminate any quad not connected to
            // an ordered quad
        {
            if (quads[i]->ordered == false) {
                bool outer = false;
                for (int j = 0; j < 4; j++) { // any neighbors that are ordered?
                    if (quads[i]->neighbors[j] && quads[i]->neighbors[j]->ordered) {
                        outer = true;
                    }
                }
                if (!outer) {	// not an outer quad, eliminate
                    PRINTF("Removing quad %d\n", i);
                    icvRemoveQuadFromGroup(quads, rcount, quads[i]);
                    rcount--;
                }
            }

        }
        return rcount;
    }

    return 0;
}


// add an outer quad
// looks for the neighbor of <quad> that isn't present,
//   tries to add it in.
// <quad> is ordered

static int
icvAddOuterQuad(CvCBQuad* quad, CvCBQuad** quads, int quad_count,
                CvCBQuad** all_quads, int all_count, CvCBCorner** corners)

{
    int added = 0;
    for (int i = 0; i < 4; i++) { // find no-neighbor corners
        if (!quad->neighbors[i]) {  // ok, create and add neighbor
            int j = (i + 2) % 4;
            PRINTF("Adding quad as neighbor 2\n");
            CvCBQuad* q = &(*all_quads)[all_count];
            memset(q, 0, sizeof(*q));
            added++;
            quads[quad_count] = q;
            quad_count++;

            // set neighbor and group id
            quad->neighbors[i] = q;
            quad->count += 1;
            q->neighbors[j] = quad;
            q->group_idx = quad->group_idx;
            q->count = 1;	// number of neighbors
            q->ordered = false;
            q->edge_len = quad->edge_len;

            // make corners of new quad
            // same as neighbor quad, but offset
            CvPoint2D32f pt = quad->corners[i]->pt;
            CvCBCorner* corner;
            float dx = pt.x - quad->corners[j]->pt.x;
            float dy = pt.y - quad->corners[j]->pt.y;
            for (int k = 0; k < 4; k++) {
                corner = &(*corners)[all_count * 4 + k];
                pt = quad->corners[k]->pt;
                memset(corner, 0, sizeof(*corner));
                corner->pt = pt;
                q->corners[k] = corner;
                corner->pt.x += dx;
                corner->pt.y += dy;
            }
            // have to set exact corner
            q->corners[j] = quad->corners[i];

            // now find other neighbor and add it, if possible
            if (quad->neighbors[(i + 3) % 4] &&
                    quad->neighbors[(i + 3) % 4]->ordered &&
                    quad->neighbors[(i + 3) % 4]->neighbors[i] &&
                    quad->neighbors[(i + 3) % 4]->neighbors[i]->ordered) {
                CvCBQuad* qn = quad->neighbors[(i + 3) % 4]->neighbors[i];
                q->count = 2;
                q->neighbors[(j + 1) % 4] = qn;
                qn->neighbors[(i + 1) % 4] = q;
                qn->count += 1;
                // have to set exact corner
                q->corners[(j + 1) % 4] = qn->corners[(i + 1) % 4];
            }

            all_count++;
        }
    }
    return added;
}


// trimming routines
#ifdef ENABLE_TRIM_COL_ROW
static int
icvTrimCol(CvCBQuad** quads, int count, int col, int dir) {
    int rcount = count;
    // find the right quad(s)
    for (int i = 0; i < count; i++) {
#ifdef DEBUG_CHESSBOARD
        if (quads[i]->ordered) {
            PRINTF("index: %d  cur: %d\n", col, quads[i]->col);
        }
#endif
        if (quads[i]->ordered && quads[i]->col == col) {
            if (dir == 1) {
                if (quads[i]->neighbors[1]) {
                    icvRemoveQuadFromGroup(quads, rcount, quads[i]->neighbors[1]);
                    rcount--;
                }
                if (quads[i]->neighbors[2]) {
                    icvRemoveQuadFromGroup(quads, rcount, quads[i]->neighbors[2]);
                    rcount--;
                }
            } else {
                if (quads[i]->neighbors[0]) {
                    icvRemoveQuadFromGroup(quads, rcount, quads[i]->neighbors[0]);
                    rcount--;
                }
                if (quads[i]->neighbors[3]) {
                    icvRemoveQuadFromGroup(quads, rcount, quads[i]->neighbors[3]);
                    rcount--;
                }
            }

        }
    }
    return rcount;
}

static int
icvTrimRow(CvCBQuad** quads, int count, int row, int dir) {
    int i, rcount = count;
    // find the right quad(s)
    for (i = 0; i < count; i++) {
#ifdef DEBUG_CHESSBOARD
        if (quads[i]->ordered) {
            PRINTF("index: %d  cur: %d\n", row, quads[i]->row);
        }
#endif
        if (quads[i]->ordered && quads[i]->row == row) {
            if (dir == 1) { // remove from bottom
                if (quads[i]->neighbors[2]) {
                    icvRemoveQuadFromGroup(quads, rcount, quads[i]->neighbors[2]);
                    rcount--;
                }
                if (quads[i]->neighbors[3]) {
                    icvRemoveQuadFromGroup(quads, rcount, quads[i]->neighbors[3]);
                    rcount--;
                }
            } else { // remove from top
                if (quads[i]->neighbors[0]) {
                    icvRemoveQuadFromGroup(quads, rcount, quads[i]->neighbors[0]);
                    rcount--;
                }
                if (quads[i]->neighbors[1]) {
                    icvRemoveQuadFromGroup(quads, rcount, quads[i]->neighbors[1]);
                    rcount--;
                }
            }

        }
    }
    return rcount;
}
#endif

//
// remove quad from quad group
//

static void
icvRemoveQuadFromGroup(CvCBQuad** quads, int count, CvCBQuad* q0) {
    int i, j;
    // remove any references to this quad as a neighbor
    for (i = 0; i < count; i++) {
        CvCBQuad* q = quads[i];
        for (j = 0; j < 4; j++) {
            if (q->neighbors[j] == q0) {
                q->neighbors[j] = 0;
                q->count--;
                for (int k = 0; k < 4; k++)
                    if (q0->neighbors[k] == q) {
                        q0->neighbors[k] = 0;
                        q0->count--;
                        break;
                    }
                break;
            }
        }
    }

    // remove the quad
    for (i = 0; i < count; i++) {
        CvCBQuad* q = quads[i];
        if (q == q0) {
            quads[i] = quads[count - 1];
            break;
        }
    }
}

//
// put quad into correct order, where <corner> has value <common>
//

static void
icvOrderQuad(CvCBQuad* quad, CvCBCorner* corner, int common) {
    // find the corner
    int tc;
    for (tc = 0; tc < 4; tc++)
        if (quad->corners[tc]->pt.x == corner->pt.x &&
                quad->corners[tc]->pt.y == corner->pt.y) {
            break;
        }

    // set corner order
    // shift
    while (tc != common) {
        // shift by one
        CvCBCorner* tempc;
        CvCBQuad* tempq;
        tempc = quad->corners[3];
        tempq = quad->neighbors[3];
        for (int i = 3; i > 0; i--) {
            quad->corners[i] = quad->corners[i - 1];
            quad->neighbors[i] = quad->neighbors[i - 1];
        }
        quad->corners[0] = tempc;
        quad->neighbors[0] = tempq;
        tc++;
        tc = tc % 4;
    }
}


// if we found too many connect quads, remove those which probably do not belong.
static int
icvCleanFoundConnectedQuads(int quad_count, CvCBQuad** quad_group, CvSize pattern_size) {
    CvPoint2D32f center = {0, 0};
    int i, j, k;
    // number of quads this pattern should contain
    int count = ((pattern_size.width + 1) * (pattern_size.height + 1) + 1) / 2;

    // if we have more quadrangles than we should,
    // try to eliminate duplicates or ones which don't belong to the pattern rectangle...
    if (quad_count <= count) {
        return quad_count;
    }

    // create an array of quadrangle centers
    cv::AutoBuffer<CvPoint2D32f> centers(quad_count);
    cv::Ptr<CvMemStorage> temp_storage = cvCreateMemStorage(0);

    for (i = 0; i < quad_count; i++) {
        CvPoint2D32f ci = {0, 0};
        CvCBQuad* q = quad_group[i];

        for (j = 0; j < 4; j++) {
            CvPoint2D32f pt = q->corners[j]->pt;
            ci.x += pt.x;
            ci.y += pt.y;
        }

        ci.x *= 0.25f;
        ci.y *= 0.25f;

        centers[i] = ci;
        center.x += ci.x;
        center.y += ci.y;
    }
    center.x /= quad_count;
    center.y /= quad_count;

    // If we still have more quadrangles than we should,
    // we try to eliminate bad ones based on minimizing the bounding box.
    // We iteratively remove the point which reduces the size of
    // the bounding box of the blobs the most
    // (since we want the rectangle to be as small as possible)
    // remove the quadrange that causes the biggest reduction
    // in pattern size until we have the correct number
    for (; quad_count > count; quad_count--) {
        double min_box_area = DBL_MAX;
        int skip, min_box_area_index = -1;
        CvCBQuad* q0, *q;

        // For each point, calculate box area without that point
        for (skip = 0; skip < quad_count; skip++) {
            // get bounding rectangle
            CvPoint2D32f temp = centers[skip]; // temporarily make index 'skip' the same as
            centers[skip] = center;            // pattern center (so it is not counted for convex hull)
            CvMat pointMat = cvMat(1, quad_count, CV_32FC2, centers);
            CvSeq* hull = cvConvexHull2(&pointMat, temp_storage, CV_CLOCKWISE, 1);
            centers[skip] = temp;
            double hull_area = fabs(cvContourArea(hull, CV_WHOLE_SEQ));

            // remember smallest box area
            if (hull_area < min_box_area) {
                min_box_area = hull_area;
                min_box_area_index = skip;
            }
            cvClearMemStorage(temp_storage);
        }

        q0 = quad_group[min_box_area_index];

        // remove any references to this quad as a neighbor
        for (i = 0; i < quad_count; i++) {
            q = quad_group[i];
            for (j = 0; j < 4; j++) {
                if (q->neighbors[j] == q0) {
                    q->neighbors[j] = 0;
                    q->count--;
                    for (k = 0; k < 4; k++)
                        if (q0->neighbors[k] == q) {
                            q0->neighbors[k] = 0;
                            q0->count--;
                            break;
                        }
                    break;
                }
            }
        }

        // remove the quad
        quad_count--;
        quad_group[min_box_area_index] = quad_group[quad_count];
        centers[min_box_area_index] = centers[quad_count];
    }

    return quad_count;
}

//=====================================================================================

static int
icvFindConnectedQuads(CvCBQuad* quad, int quad_count, CvCBQuad** out_group,
                      int group_idx, CvMemStorage* storage) {
    cv::Ptr<CvMemStorage> temp_storage = cvCreateChildMemStorage(storage);
    CvSeq* stack = cvCreateSeq(0, sizeof(*stack), sizeof(void*), temp_storage);
    int i, count = 0;

    // Scan the array for a first unlabeled quad
    for (i = 0; i < quad_count; i++) {
        if (quad[i].count > 0 && quad[i].group_idx < 0) {
            break;
        }
    }

    // Recursively find a group of connected quads starting from the seed quad[i]
    if (i < quad_count) {
        CvCBQuad* q = &quad[i];
        cvSeqPush(stack, &q);
        out_group[count++] = q;
        q->group_idx = group_idx;
        q->ordered = false;

        while (stack->total) {
            cvSeqPop(stack, &q);
            for (i = 0; i < 4; i++) {
                CvCBQuad* neighbor = q->neighbors[i];
                if (neighbor && neighbor->count > 0 && neighbor->group_idx < 0) {
                    cvSeqPush(stack, &neighbor);
                    out_group[count++] = neighbor;
                    neighbor->group_idx = group_idx;
                    neighbor->ordered = false;
                }
            }
        }
    }

    return count;
}


//=====================================================================================

static int
icvCheckQuadGroup(CvCBQuad** quad_group, int quad_count,
                  CvCBCorner** out_corners, CvSize pattern_size) {
    const int ROW1 = 1000000;
    const int ROW2 = 2000000;
    const int ROW_ = 3000000;
    int result = 0;
    int i, out_corner_count = 0, corner_count = 0;
    std::vector<CvCBCorner*> corners(quad_count * 4);

    int j, k, kk;
    int width = 0, height = 0;
    int hist[5] = {0, 0, 0, 0, 0};
    CvCBCorner* first = 0, *first2 = 0, *right, *cur, *below, *c;

    // build dual graph, which vertices are internal quad corners
    // and two vertices are connected iff they lie on the same quad edge
    for (i = 0; i < quad_count; i++) {
        CvCBQuad* q = quad_group[i];
        /*CvScalar color = q->count == 0 ? cvScalar(0,255,255) :
                         q->count == 1 ? cvScalar(0,0,255) :
                         q->count == 2 ? cvScalar(0,255,0) :
                         q->count == 3 ? cvScalar(255,255,0) :
                                         cvScalar(255,0,0);*/

        for (j = 0; j < 4; j++) {
            //cvLine( debug_img, cvPointFrom32f(q->corners[j]->pt), cvPointFrom32f(q->corners[(j+1)&3]->pt), color, 1, CV_AA, 0 );
            if (q->neighbors[j]) {
                CvCBCorner* a = q->corners[j], *b = q->corners[(j + 1) & 3];
                // mark internal corners that belong to:
                //   - a quad with a single neighbor - with ROW1,
                //   - a quad with two neighbors     - with ROW2
                // make the rest of internal corners with ROW_
                int row_flag = q->count == 1 ? ROW1 : q->count == 2 ? ROW2 : ROW_;

                if (a->row == 0) {
                    corners[corner_count++] = a;
                    a->row = row_flag;
                } else if (a->row > row_flag) {
                    a->row = row_flag;
                }

                if (q->neighbors[(j + 1) & 3]) {
                    if (a->count >= 4 || b->count >= 4) {
                        goto finalize;
                    }
                    for (k = 0; k < 4; k++) {
                        if (a->neighbors[k] == b) {
                            goto finalize;
                        }
                        if (b->neighbors[k] == a) {
                            goto finalize;
                        }
                    }
                    a->neighbors[a->count++] = b;
                    b->neighbors[b->count++] = a;
                }
            }
        }
    }

    if (corner_count != pattern_size.width * pattern_size.height) {
        goto finalize;
    }

    for (i = 0; i < corner_count; i++) {
        int n = corners[i]->count;
        assert(0 <= n && n <= 4);
        hist[n]++;
        if (!first && n == 2) {
            if (corners[i]->row == ROW1) {
                first = corners[i];
            } else if (!first2 && corners[i]->row == ROW2) {
                first2 = corners[i];
            }
        }
    }

    // start with a corner that belongs to a quad with a signle neighbor.
    // if we do not have such, start with a corner of a quad with two neighbors.
    if (!first) {
        first = first2;
    }

    if (!first || hist[0] != 0 || hist[1] != 0 || hist[2] != 4 ||
            hist[3] != (pattern_size.width + pattern_size.height) * 2 - 8) {
        goto finalize;
    }

    cur = first;
    right = below = 0;
    out_corners[out_corner_count++] = cur;

    for (k = 0; k < 4; k++) {
        c = cur->neighbors[k];
        if (c) {
            if (!right) {
                right = c;
            } else if (!below) {
                below = c;
            }
        }
    }

    if (!right || (right->count != 2 && right->count != 3) ||
            !below || (below->count != 2 && below->count != 3)) {
        goto finalize;
    }

    cur->row = 0;
    //cvCircle( debug_img, cvPointFrom32f(cur->pt), 3, cvScalar(0,255,0), -1, 8, 0 );

    first = below; // remember the first corner in the next row
    // find and store the first row (or column)
    for (j = 1;; j++) {
        right->row = 0;
        out_corners[out_corner_count++] = right;
        //cvCircle( debug_img, cvPointFrom32f(right->pt), 3, cvScalar(0,255-j*10,0), -1, 8, 0 );
        if (right->count == 2) {
            break;
        }
        if (right->count != 3 || out_corner_count >= MAX(pattern_size.width, pattern_size.height)) {
            goto finalize;
        }
        cur = right;
        for (k = 0; k < 4; k++) {
            c = cur->neighbors[k];
            if (c && c->row > 0) {
                for (kk = 0; kk < 4; kk++) {
                    if (c->neighbors[kk] == below) {
                        break;
                    }
                }
                if (kk < 4) {
                    below = c;
                } else {
                    right = c;
                }
            }
        }
    }

    width = out_corner_count;
    if (width == pattern_size.width) {
        height = pattern_size.height;
    } else if (width == pattern_size.height) {
        height = pattern_size.width;
    } else {
        goto finalize;
    }

    // find and store all the other rows
    for (i = 1; ; i++) {
        if (!first) {
            break;
        }
        cur = first;
        first = 0;
        for (j = 0;; j++) {
            cur->row = i;
            out_corners[out_corner_count++] = cur;
            //cvCircle( debug_img, cvPointFrom32f(cur->pt), 3, cvScalar(0,0,255-j*10), -1, 8, 0 );
            if (cur->count == 2 + (i < height - 1) && j > 0) {
                break;
            }

            right = 0;

            // find a neighbor that has not been processed yet
            // and that has a neighbor from the previous row
            for (k = 0; k < 4; k++) {
                c = cur->neighbors[k];
                if (c && c->row > i) {
                    for (kk = 0; kk < 4; kk++) {
                        if (c->neighbors[kk] && c->neighbors[kk]->row == i - 1) {
                            break;
                        }
                    }
                    if (kk < 4) {
                        right = c;
                        if (j > 0) {
                            break;
                        }
                    } else if (j == 0) {
                        first = c;
                    }
                }
            }
            if (!right) {
                goto finalize;
            }
            cur = right;
        }

        if (j != width - 1) {
            goto finalize;
        }
    }

    if (out_corner_count != corner_count) {
        goto finalize;
    }

    // check if we need to transpose the board
    if (width != pattern_size.width) {
        CV_SWAP(width, height, k);

        memcpy(&corners[0], out_corners, corner_count * sizeof(corners[0]));
        for (i = 0; i < height; i++)
            for (j = 0; j < width; j++) {
                out_corners[i * width + j] = corners[j * height + i];
            }
    }

    // check if we need to revert the order in each row
    {
        CvPoint2D32f p0 = out_corners[0]->pt, p1 = out_corners[pattern_size.width - 1]->pt,
                     p2 = out_corners[pattern_size.width]->pt;
        if ((p1.x - p0.x) * (p2.y - p1.y) - (p1.y - p0.y) * (p2.x - p1.x) < 0) {
            if (width % 2 == 0) {
                for (i = 0; i < height; i++)
                    for (j = 0; j < width / 2; j++) {
                        CV_SWAP(out_corners[i * width + j], out_corners[i * width + width - j - 1], c);
                    }
            } else {
                for (j = 0; j < width; j++)
                    for (i = 0; i < height / 2; i++) {
                        CV_SWAP(out_corners[i * width + j], out_corners[(height - i - 1)*width + j], c);
                    }
            }
        }
    }

    result = corner_count;

finalize:

    if (result <= 0) {
        corner_count = MIN(corner_count, pattern_size.width * pattern_size.height);
        for (i = 0; i < corner_count; i++) {
            out_corners[i] = corners[i];
        }
        result = -corner_count;

        if (result == -pattern_size.width * pattern_size.height) {
            result = -result;
        }
    }

    return result;
}




//=====================================================================================

static void icvFindQuadNeighbors(CvCBQuad* quads, int quad_count) {
    const float thresh_scale = 1.f;
    int idx, i, k, j;
    float dx, dy, dist;

    // find quad neighbors
    for (idx = 0; idx < quad_count; idx++) {
        CvCBQuad* cur_quad = &quads[idx];

        // choose the points of the current quadrangle that are close to
        // some points of the other quadrangles
        // (it can happen for split corners (due to dilation) of the
        // checker board). Search only in other quadrangles!

        // for each corner of this quadrangle
        for (i = 0; i < 4; i++) {
            CvPoint2D32f pt;
            float min_dist = FLT_MAX;
            int closest_corner_idx = -1;
            CvCBQuad* closest_quad = 0;
            CvCBCorner* closest_corner = 0;

            if (cur_quad->neighbors[i]) {
                continue;
            }

            pt = cur_quad->corners[i]->pt;

            // find the closest corner in all other quadrangles
            for (k = 0; k < quad_count; k++) {
                if (k == idx) {
                    continue;
                }

                for (j = 0; j < 4; j++) {
                    if (quads[k].neighbors[j]) {
                        continue;
                    }

                    dx = pt.x - quads[k].corners[j]->pt.x;
                    dy = pt.y - quads[k].corners[j]->pt.y;
                    dist = dx * dx + dy * dy;

                    if (dist < min_dist &&
                            dist <= cur_quad->edge_len * thresh_scale &&
                            dist <= quads[k].edge_len * thresh_scale) {
                        // check edge lengths, make sure they're compatible
                        // edges that are different by more than 1:4 are rejected
                        float ediff = cur_quad->edge_len - quads[k].edge_len;
                        if (ediff > 32 * cur_quad->edge_len ||
                                ediff > 32 * quads[k].edge_len) {
                            PRINTF("Incompatible edge lengths\n");
                            continue;
                        }
                        closest_corner_idx = j;
                        closest_quad = &quads[k];
                        min_dist = dist;
                    }
                }
            }

            // we found a matching corner point?
            if (closest_corner_idx >= 0 && min_dist < FLT_MAX) {
                // If another point from our current quad is closer to the found corner
                // than the current one, then we don't count this one after all.
                // This is necessary to support small squares where otherwise the wrong
                // corner will get matched to closest_quad;
                closest_corner = closest_quad->corners[closest_corner_idx];

                for (j = 0; j < 4; j++) {
                    if (cur_quad->neighbors[j] == closest_quad) {
                        break;
                    }

                    dx = closest_corner->pt.x - cur_quad->corners[j]->pt.x;
                    dy = closest_corner->pt.y - cur_quad->corners[j]->pt.y;

                    if (dx * dx + dy * dy < min_dist) {
                        break;
                    }
                }

                if (j < 4 || cur_quad->count >= 4 || closest_quad->count >= 4) {
                    continue;
                }

                // Check that each corner is a neighbor of different quads
                for (j = 0; j < closest_quad->count; j++) {
                    if (closest_quad->neighbors[j] == cur_quad) {
                        break;
                    }
                }
                if (j < closest_quad->count) {
                    continue;
                }

                // check whether the closest corner to closest_corner
                // is different from cur_quad->corners[i]->pt
                for (k = 0; k < quad_count; k++) {
                    CvCBQuad* q = &quads[k];
                    if (k == idx || q == closest_quad) {
                        continue;
                    }

                    for (j = 0; j < 4; j++)
                        if (!q->neighbors[j]) {
                            dx = closest_corner->pt.x - q->corners[j]->pt.x;
                            dy = closest_corner->pt.y - q->corners[j]->pt.y;
                            dist = dx * dx + dy * dy;
                            if (dist < min_dist) {
                                break;
                            }
                        }
                    if (j < 4) {
                        break;
                    }
                }

                if (k < quad_count) {
                    continue;
                }

                closest_corner->pt.x = (pt.x + closest_corner->pt.x) * 0.5f;
                closest_corner->pt.y = (pt.y + closest_corner->pt.y) * 0.5f;

                // We've found one more corner - remember it
                cur_quad->count++;
                cur_quad->neighbors[i] = closest_quad;
                cur_quad->corners[i] = closest_corner;

                closest_quad->count++;
                closest_quad->neighbors[closest_corner_idx] = cur_quad;
            }
        }
    }
}

//=====================================================================================

// returns corners in clockwise order
// corners don't necessarily start at same position on quad (e.g.,
//   top left corner)

static int
icvGenerateQuads(CvCBQuad** out_quads, CvCBCorner** out_corners,
                 CvMemStorage* storage, CvMat* image, int flags) {
    int quad_count = 0;
    cv::Ptr<CvMemStorage> temp_storage;

    if (out_quads) {
        *out_quads = 0;
    }

    if (out_corners) {
        *out_corners = 0;
    }

    CvSeq* src_contour = 0;
    CvSeq* root;
    CvContourEx* board = 0;
    CvContourScanner scanner;
    int i, idx, min_size;

    CV_Assert(out_corners && out_quads);

    // empiric bound for minimal allowed perimeter for squares
    min_size = 25; //cvRound( image->cols * image->rows * .03 * 0.01 * 0.92 );

    // create temporary storage for contours and the sequence of pointers to found quadrangles
    temp_storage = cvCreateChildMemStorage(storage);
    root = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvSeq*), temp_storage);

    // initialize contour retrieving routine
    scanner = cvStartFindContours(image, temp_storage, sizeof(CvContourEx),
                                  CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);

    // get all the contours one by one
    while ((src_contour = cvFindNextContour(scanner)) != 0) {
        CvSeq* dst_contour = 0;
        CvRect rect = ((CvContour*)src_contour)->rect;

        // reject contours with too small perimeter
        if (CV_IS_SEQ_HOLE(src_contour) && rect.width* rect.height >= min_size) {
            const int min_approx_level = 2, max_approx_level = MAX_CONTOUR_APPROX;
            int approx_level;
            for (approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++) {
                dst_contour = cvApproxPoly(src_contour, sizeof(CvContour), temp_storage,
                                           CV_POLY_APPROX_DP, (float)approx_level);
                // we call this again on its own output, because sometimes
                // cvApproxPoly() does not simplify as much as it should.
                dst_contour = cvApproxPoly(dst_contour, sizeof(CvContour), temp_storage,
                                           CV_POLY_APPROX_DP, (float)approx_level);

                if (dst_contour->total == 4) {
                    break;
                }
            }

            // reject non-quadrangles
            if (dst_contour->total == 4 && cvCheckContourConvexity(dst_contour)) {
                CvPoint pt[4];
                double d1, d2, p = cvContourPerimeter(dst_contour);
                double area = fabs(cvContourArea(dst_contour, CV_WHOLE_SEQ));
                double dx, dy;

                for (i = 0; i < 4; i++) {
                    pt[i] = *(CvPoint*)cvGetSeqElem(dst_contour, i);
                }

                dx = pt[0].x - pt[2].x;
                dy = pt[0].y - pt[2].y;
                d1 = sqrt(dx * dx + dy * dy);

                dx = pt[1].x - pt[3].x;
                dy = pt[1].y - pt[3].y;
                d2 = sqrt(dx * dx + dy * dy);

                // philipg.  Only accept those quadrangles which are more square
                // than rectangular and which are big enough
                double d3, d4;
                dx = pt[0].x - pt[1].x;
                dy = pt[0].y - pt[1].y;
                d3 = sqrt(dx * dx + dy * dy);
                dx = pt[1].x - pt[2].x;
                dy = pt[1].y - pt[2].y;
                d4 = sqrt(dx * dx + dy * dy);
                if (!(flags & CV_CALIB_CB_FILTER_QUADS) ||
                        (d3 * 4 > d4 && d4 * 4 > d3 && d3 * d4 < area * 1.5 && area > min_size &&
                         d1 >= 0.15 * p && d2 >= 0.15 * p)) {
                    CvContourEx* parent = (CvContourEx*)(src_contour->v_prev);
                    parent->counter++;
                    if (!board || board->counter < parent->counter) {
                        board = parent;
                    }
                    dst_contour->v_prev = (CvSeq*)parent;
                    //for( i = 0; i < 4; i++ ) cvLine( debug_img, pt[i], pt[(i+1)&3], cvScalar(200,255,255), 1, CV_AA, 0 );
                    cvSeqPush(root, &dst_contour);
                }
            }
        }
    }

    // finish contour retrieving
    cvEndFindContours(&scanner);

    // allocate quad & corner buffers
    *out_quads = (CvCBQuad*)cvAlloc((root->total + root->total / 2) * sizeof((*out_quads)[0]));
    *out_corners = (CvCBCorner*)cvAlloc((root->total + root->total / 2) * 4 * sizeof((*out_corners)[0]));

    // Create array of quads structures
    for (idx = 0; idx < root->total; idx++) {
        CvCBQuad* q = &(*out_quads)[quad_count];
        src_contour = *(CvSeq**)cvGetSeqElem(root, idx);
        if ((flags & CV_CALIB_CB_FILTER_QUADS) && src_contour->v_prev != (CvSeq*)board) {
            continue;
        }

        // reset group ID
        memset(q, 0, sizeof(*q));
        q->group_idx = -1;
        assert(src_contour->total == 4);
        for (i = 0; i < 4; i++) {
            CvPoint2D32f pt = cvPointTo32f(*(CvPoint*)cvGetSeqElem(src_contour, i));
            CvCBCorner* corner = &(*out_corners)[quad_count * 4 + i];

            memset(corner, 0, sizeof(*corner));
            corner->pt = pt;
            q->corners[i] = corner;
        }
        q->edge_len = FLT_MAX;
        for (i = 0; i < 4; i++) {
            float dx = q->corners[i]->pt.x - q->corners[(i + 1) & 3]->pt.x;
            float dy = q->corners[i]->pt.y - q->corners[(i + 1) & 3]->pt.y;
            float d = dx * dx + dy * dy;
            if (q->edge_len > d) {
                q->edge_len = d;
            }
        }
        quad_count++;
    }

    return quad_count;
}


CV_IMPL void
cvDrawChessboardCorners(CvArr* _image, CvSize pattern_size,
                        CvPoint2D32f* corners, int count, int found) {
    const int shift = 0;
    const int radius = 4;
    const int r = radius * (1 << shift);
    int i;
    CvMat stub, *image;
    double scale = 1;
    int type, cn, line_type;

    image = cvGetMat(_image, &stub);

    type = CV_MAT_TYPE(image->type);
    cn = CV_MAT_CN(type);
    if (cn != 1 && cn != 3 && cn != 4) {
        CV_Error(CV_StsUnsupportedFormat, "Number of channels must be 1, 3 or 4");
    }

    switch (CV_MAT_DEPTH(image->type)) {
    case CV_8U:
        scale = 1;
        break;
    case CV_16U:
        scale = 256;
        break;
    case CV_32F:
        scale = 1. / 255;
        break;
    default:
        CV_Error(CV_StsUnsupportedFormat,
                 "Only 8-bit, 16-bit or floating-point 32-bit images are supported");
    }

    line_type = type == CV_8UC1 || type == CV_8UC3 ? CV_AA : 8;

    if (!found) {
        CvScalar color = {{0, 0, 255}};
        if (cn == 1) {
            color = cvScalarAll(200);
        }
        color.val[0] *= scale;
        color.val[1] *= scale;
        color.val[2] *= scale;
        color.val[3] *= scale;

        for (i = 0; i < count; i++) {
            CvPoint pt;
            pt.x = cvRound(corners[i].x * (1 << shift));
            pt.y = cvRound(corners[i].y * (1 << shift));
            cvLine(image, cvPoint(pt.x - r, pt.y - r),
                   cvPoint(pt.x + r, pt.y + r), color, 1, line_type, shift);
            cvLine(image, cvPoint(pt.x - r, pt.y + r),
                   cvPoint(pt.x + r, pt.y - r), color, 1, line_type, shift);
            cvCircle(image, pt, r + (1 << shift), color, 1, line_type, shift);
        }
    } else {
        int x, y;
        CvPoint prev_pt = {0, 0};
        const int line_max = 7;
        static const CvScalar line_colors[line_max] = {
            {{0, 0, 255}},
            {{0, 128, 255}},
            {{0, 200, 200}},
            {{0, 255, 0}},
            {{200, 200, 0}},
            {{255, 0, 0}},
            {{255, 0, 255}}
        };

        for (y = 0, i = 0; y < pattern_size.height; y++) {
            CvScalar color = line_colors[y % line_max];
            if (cn == 1) {
                color = cvScalarAll(200);
            }
            color.val[0] *= scale;
            color.val[1] *= scale;
            color.val[2] *= scale;
            color.val[3] *= scale;

            for (x = 0; x < pattern_size.width; x++, i++) {
                CvPoint pt;
                pt.x = cvRound(corners[i].x * (1 << shift));
                pt.y = cvRound(corners[i].y * (1 << shift));

                if (i != 0) {
                    cvLine(image, prev_pt, pt, color, 1, line_type, shift);
                }

                cvLine(image, cvPoint(pt.x - r, pt.y - r),
                       cvPoint(pt.x + r, pt.y + r), color, 1, line_type, shift);
                cvLine(image, cvPoint(pt.x - r, pt.y + r),
                       cvPoint(pt.x + r, pt.y - r), color, 1, line_type, shift);
                cvCircle(image, pt, r + (1 << shift), color, 1, line_type, shift);
                prev_pt = pt;
            }
        }
    }
}

namespace cv {

bool findChessboardCorners(const Mat& image, Size patternSize,
                           vector<Point2f>& corners, int flags) {
    int count = patternSize.area() * 2;
    corners.resize(count);
    CvMat _image = image;
    bool ok = cvFindChessboardCorners(&_image, patternSize,
                                      (CvPoint2D32f*)&corners[0], &count, flags) > 0;
    corners.resize(count);
    return ok;
}

void drawChessboardCorners(Mat& image, Size patternSize,
                           const Mat& corners,
                           bool patternWasFound) {
    if (corners.cols == 0 || corners.rows == 0) {
        return;
    }
    CvMat _image = image;
    CV_Assert((corners.cols == 1 || corners.rows == 1) &&
              corners.type() == CV_32FC2 && corners.isContinuous());
    cvDrawChessboardCorners(&_image, patternSize, (CvPoint2D32f*)corners.data,
                            corners.cols + corners.rows - 1, patternWasFound);
}

}

/* End of file. */
